/**********************************************************************
*  Copyright (c) 2007 - Victor Jacobs - victor.jacobs@gmail.com
*
*  Permission is hereby granted, free of charge, to any person
*  obtaining a copy of this software and associated documentation
*  files (the "Software"), to deal in the Software without
*  restriction, including without limitation the rights to use,
*  copy, modify, merge, publish, distribute, sublicense, and/or sell
*  copies of the Software, and to permit persons to whom the
*  Software is furnished to do so, subject to the following
*  conditions:
*
*  The above copyright notice and this permission notice shall be
*  included in all copies or substantial portions of the Software.
*
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
*  OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
*  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
*  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
*  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
*  OTHER DEALINGS IN THE SOFTWARE.
**********************************************************************/

using System;
using System.Collections.Generic;
using System.Text;
using Tao.Ode;
using Vj.MathLibrary;

namespace Vj.OdeObjectLibrary
{
    public class ODEUtility
    {
        public static Vector dVector3_to_Vector(Ode.dVector3 inVector)
        {
            Vector returnVector = new Vector(3);
            returnVector.X = inVector.X;
            returnVector.Y = inVector.Y;
            returnVector.Z = inVector.Z;
            return returnVector;
        }

        public static Ode.dVector3 Vector_to_dVector3(Vector inVector)
        {
            Ode.dVector3 returnVector = new Ode.dVector3();
            returnVector.X = (float)inVector.X;
            returnVector.Y = (float)inVector.Y;
            returnVector.Z = (float)inVector.Z;
            return returnVector;
        }

        //public static Ode.dMatrix3 Matrix_to_dMatrix3()
        //{ 
            
        
        
        //}

        public static Ode.dVector3 Add(Ode.dVector3 v1, Ode.dVector3 v2)
        {
            Ode.dVector3 returnVector = new Ode.dVector3();

            returnVector.X = v1.X + v2.X;
            returnVector.Y = v1.Y + v2.Y;
            returnVector.Z = v1.Z + v2.Z;

            return returnVector;
        }

        public static Ode.dVector3 Subtract(Ode.dVector3 v1, Ode.dVector3 v2)
        {
            Ode.dVector3 returnVector = new Ode.dVector3();

            returnVector.X = v1.X - v2.X;
            returnVector.Y = v1.Y - v2.Y;
            returnVector.Z = v1.Z - v2.Z;

            return returnVector;
        }

  

        public static float[] ODE_Rotation_Position_to_OpenGL_FloatArray(Ode.dMatrix3 rotMatrix, Ode.dVector3 posVector)
        {
            float[] returnMatrix = new float[16];

            returnMatrix[0] = rotMatrix[0];
            returnMatrix[1] = rotMatrix[4];
            returnMatrix[2] = rotMatrix[8];
            returnMatrix[3] = 0.0f;
            
            returnMatrix[4] = rotMatrix[1];
            returnMatrix[5] = rotMatrix[5];
            returnMatrix[6] = rotMatrix[9];
            returnMatrix[7] = 0.0f;

            returnMatrix[8] = rotMatrix[2];
            returnMatrix[9] = rotMatrix[6];
            returnMatrix[10] = rotMatrix[10];
            returnMatrix[11] = 0.0f;

            returnMatrix[12] = posVector.X;
            returnMatrix[13] = posVector.Y;
            returnMatrix[14] = posVector.Z;
            returnMatrix[15] = 1.0f;

            return returnMatrix;
        }



    }
}
